import numpy as np
import acado
import time
import matplotlib.pyplot as plt
from numpy import sin, cos,exp
from mpl_toolkits.mplot3d import Axes3D

def weighted_center_of_pc(p_body,pc_W):
    pc_B=pc_W-p_body
    dist_2=np.sum(pc_B*pc_B,axis=0)
    dist=np.sqrt(dist_2)

    weight=(1/dist)/np.sum(1/dist)
    pc_W_weighted=weight*pc_W
    pc_W_weighted_center=np.sum(pc_W_weighted,axis=1)
    print(pc_W_weighted_center)

    return pc_W_weighted_center

a=0.666
pc_W=np.load("../ov_pc_W_left_bottom corner.npy")



fx=pc_W[0,:]*0+10+np.random.randn(pc_W.shape[1])*0
fy=pc_W[1,:]*0+10+np.random.randn(pc_W.shape[1])*0
fz=pc_W[2,:]*0+1.1+np.random.randn(pc_W.shape[1])*0


p5_deg_arr=np.arange(-90,90,2)
p6_deg_arr=np.arange(-90,90,2)

p5_rad_arr=np.deg2rad(p5_deg_arr)
p6_rad_arr=np.deg2rad(p6_deg_arr)

cost_arr=np.zeros([p5_rad_arr.shape[0],p6_rad_arr.shape[0]])

bx=0
by=0
bz=0

for i5 in range(p5_rad_arr.shape[0]):
    for i6 in range(p6_rad_arr.shape[0]):
        cost=0

        p_5=p5_rad_arr[i5]
        p_6=p6_rad_arr[i6]

        view_x=cos(p_5)*cos(p_6);
        view_y=sin(p_6);
        view_z=-sin(p_5)*cos(p_6);

        for ipc in range(pc_W.shape[1]):
            fx_B=fx[ipc]-bx;
            fy_B=fy[ipc]-by;
            fz_B=fz[ipc]-bz;
            f_dist=np.sqrt((fx_B**2)+(fy_B**2)+(fz_B**2))
            cos_bear=(view_x*fx_B+view_y*fy_B+view_z*fz_B)/f_dist
            bear=np.arccos(cos_bear)
            visibility=1/exp(-a*cos(bear)+1)
            
            cost+=bear/f_dist

        #print("i5:"+str(i5)+" i6: "+str(i6))
        
        cost_arr[i5,i6]=cost
            
min_cost_id=np.where(cost_arr==np.max(cost_arr))



fig=plt.figure()
ax1=plt.axes(projection='3d')


ax1.set_xlabel('X')
ax1.set_ylabel('Y')
ax1.set_zlabel('Z')
ax1.scatter(fx,fy,fz,cmap='Blues')
plt.show()

print(min_cost_id)

print(p5_deg_arr[min_cost_id[0][0]])
print(p6_deg_arr[min_cost_id[1][0]])


fig=plt.figure()
ax3=plt.axes(projection='3d')
ax3.set_xlabel('X')
ax3.set_ylabel('Y')
ax3.set_zlabel('Z')
        
X,Y=np.meshgrid(p5_deg_arr,p6_deg_arr)
Z=cost_arr
ax3.plot_surface(X,Y,Z,cmap='rainbow') 
plt.title("3D")
plt.show()